// Opencv相关库
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <cv.h>
#include <highgui.h>

// Ros相关库
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Point.h>
#include <acupoint_msgs/AcupointImagePixel.h>
#include <acupoint_msgs/AcupointCoordinate.h>

// stl相关库
#include <iostream>
#include <vector>
#include <string>
#include <mutex>
#include <cmath>


// 穴位像素坐标结构体
struct ImagePixel{
    int x;
    int y;
    std::string name;
};

// 深度相机内参
const double camera_factor = 1;
const double camera_cx = 1022.65;
const double camera_cy = 773.947;
const double camera_fx = 974.02;
const double camera_fy = 973.636;

std::mutex m;
std::vector<ImagePixel> image_pixels;

cv_bridge::CvImagePtr depth_ptr;
cv::Mat depth_pic;

// 深度图回调函数
void depthCallback(const sensor_msgs::ImageConstPtr &depth_msg)
{
    std::lock_guard<std::mutex> ld(m);
    try
    {
        depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1);
        ROS_INFO("convert depth successs");
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("depthCallback:Could not convert from '%s' to 'mono16'.", depth_msg->encoding.c_str());
    }
    depth_pic = depth_ptr->image;

    // output some info about the depth image in cv format
    // cout << "output some info about the depth image in cv format" << endl;
    // cout << "rows of the depth image = " << depth_pic.rows << endl;
    // cout << "cols of the depth image = " << depth_pic.cols << endl;
    // cout << "type of depth_pic's element = " << depth_pic.type() << endl;
}



// 像素坐标回调函数
void pixelCallback(const acupoint_msgs::AcupointImagePixelConstPtr &msg){
    std::lock_guard<std::mutex> ld(m);
    if(msg->name.size()!=image_pixels.size()){
        image_pixels.resize(msg->name.size());
        for(int i=0;i<image_pixels.size();++i){
            image_pixels[i].name=msg->name[i];
        }
    }
    for(int i=0;i<image_pixels.size();++i){
        image_pixels[i].x=std::round(msg->pixels[i].x);
        image_pixels[i].y=std::round(msg->pixels[i].y);
    }
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "acu2camera_coordinate_node");
    ros::NodeHandle nh;

    image_transport::ImageTransport it(nh);
    image_transport::Subscriber depth_sub = it.subscribe("/k4a/depth/depth_to_color", 1, depthCallback);
    ros::Publisher acupoint_pub=nh.advertise<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2camera_coordinate",100);
    ros::Subscriber pixel_sub=nh.subscribe<acupoint_msgs::AcupointImagePixel>("/acupoint/acu_filter_image_pixel",100,pixelCallback);
    
    ros::AsyncSpinner spinner(2);
    spinner.start();

    std::cout<<"init completed\n";
    ros::Duration(2).sleep();
    ros::Rate rate(3); 
    
    int image_x,image_y;

    acupoint_msgs::AcupointCoordinate acu_coordinate_msg;
    bool pub_flag=false;
    while (ros::ok())
    {
        std::cout<<"looping\n";
        {
            std::lock_guard<std::mutex> ld(m);

            // 将像素坐标和笛卡尔坐标数组对齐
            if(acu_coordinate_msg.name.size()!=image_pixels.size()){
                acu_coordinate_msg.name.resize(image_pixels.size());
                acu_coordinate_msg.acupoints.resize(image_pixels.size());
                for(int i=0;i<acu_coordinate_msg.name.size();++i){
                    acu_coordinate_msg.name[i]=image_pixels[i].name;
                }
            }
            pub_flag=false;
            // 利用相似三角形性质将深度图像转换为笛卡尔坐标
            for(int i=0;i<image_pixels.size();++i){
                image_x=image_pixels[i].x;
                image_y=image_pixels[i].y;
                if(image_x>=0 && image_x<depth_pic.rows && image_y>=0 && image_y<depth_pic.cols){
                    pub_flag=true; //fix：当人不在相机视野时，可能会发布历史穴位坐标数据
                    double d = depth_pic.ptr<float>(image_x)[image_y];
                    acu_coordinate_msg.acupoints[i].z = double(d) / camera_factor;
                    acu_coordinate_msg.acupoints[i].x = (image_y - camera_cx) * acu_coordinate_msg.acupoints[i].z / camera_fx;
                    acu_coordinate_msg.acupoints[i].y = (image_x - camera_cy) * acu_coordinate_msg.acupoints[i].z / camera_fy;
                }
            }

            if(pub_flag){
                // 发布笛卡尔坐标
                acu_coordinate_msg.header.stamp=ros::Time::now();
                acupoint_pub.publish(acu_coordinate_msg);
            }
            

            // 将穴位坐标置为-1
            for(int i=0;i<image_pixels.size();++i){
                image_pixels[i].x=-1;
                image_pixels[i].y=-1;
            }
        }
        std::cout<<"loop end\n";
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
